#include "pulsingEngine.h"

buff_type prv = {
  Origin_X,Origin_Y,Origin_Z,Origin_A,Origin_B,Origin_C,0}
,curr = {
  Origin_X,Origin_Y,Origin_Z,Origin_A,Origin_B,Origin_C,0};   //assign home coordinates to both
char sig_step[6] = {
  0,0,0,0,0,0};

char sig_dir[6] = {
  1,1,1,1,1,1};

int p[6] ={
  0,0,0,0,0,0};

//int pos = Origin_X;

void pulseInit(uint32 period,uint32 compareAt)
{  
  uint64 cmpCnt;
  uint16 overflow;  

  Timer2.setMode(TIMER_COMRPARE_CH_1,TIMER_OUTPUT_COMPARE);
  Timer2.setMode(TIMER_COMRPARE_CH_2,TIMER_OUTPUT_COMPARE);

  overflow = Timer2.setPeriod(period); // in microseconds
  cmpCnt = (overflow * compareAt)/ period;

  Timer2.setCompare(TIMER_COMRPARE_CH_1,(uint16)cmpCnt);
  Timer2.setCompare(TIMER_COMRPARE_CH_2,1);
  Timer2.attachInterrupt(TIMER_COMRPARE_CH_1,handler_compareAt);
  Timer2.attachInterrupt(TIMER_COMRPARE_CH_2,handler_period);

  Timer2.refresh();
  Timer2.resume();
}

void reAssignTimers(uint32 period,uint32 compareAt)
{
  uint16 overflow;
  uint64 cmpCnt;

  overflow = Timer2.setPeriod(period); // in microseconds
  cmpCnt = (overflow * compareAt)/ period;

  Timer2.setCompare(TIMER_COMRPARE_CH_1,cmpCnt);
  Timer2.setCompare(TIMER_COMRPARE_CH_2,1);  
}

void handler_compareAt()
{
  int diff;
  uint32 newPeriod;
  //set values of pulse output ports
  digitalWrite(STEPPinX,0);
  digitalWrite(STEPPinY,0);
  digitalWrite(STEPPinZ,0);
  digitalWrite(STEPPinA,0);
  digitalWrite(STEPPinB,0);
  digitalWrite(STEPPinC,0);

  if(fifo_isempty())
  {
    sig_step[0] = sig_step[1] = sig_step[2] = sig_step[3] = sig_step[4] = sig_step[5] = 0;
    sig_dir[0] = !dirx;
    sig_dir[1] = !diry;
    sig_dir[2] = !dirz;
    sig_dir[3] = !dira;
    sig_dir[4] = !dirb;
    sig_dir[5] = !dirc;
    return;
  }

  prv = curr;
  curr = fifo_remove();
    /*
  SerialUSB.print(" curr.x = ");
  SerialUSB.print(curr.x);
  SerialUSB.print(" curr.y = ");
  SerialUSB.print(curr.y);
  SerialUSB.print(" curr.z = ");
  SerialUSB.print(curr.z);
  SerialUSB.print(" curr.a = ");
  SerialUSB.print(curr.a);
  SerialUSB.print(" curr.b = ");
  SerialUSB.print(curr.b);
  SerialUSB.print(" curr.c = ");
  SerialUSB.print(curr.c);
  SerialUSB.print(" curr.Speed = ");
  SerialUSB.println(curr.Speed);
  */
  //set step and direction 
  diff = curr.x - prv.x;
  p[0] = p[0] + diff;
  if((abs(p[0])) >= micronperpulseX)
  {
    sig_step[0] = 1;
    if(p[0]<0)
      sig_dir[0] = !dirx;
    else
        sig_dir[0] = dirx;    
    p[0] = 0;    
  }
  else  
    sig_step[0] = 0;

  diff = curr.y - prv.y;
  p[1] = p[1] + diff;
  if((abs(p[1])) >= micronperpulseY)
  {
    sig_step[1] = 1;
    if(p[1]<0)
      sig_dir[1] = !diry;
    else
        sig_dir[1] = diry;            
    p[1] = 0;
  }
  else
    sig_step[1] = 0;
    
  diff = curr.z - prv.z;
  p[2] = p[2] + diff;
  if((abs(p[2])) >= micronperpulseZ)
  {
    sig_step[2] = 1;
    if(p[2]<0)
      sig_dir[2] = !dirz;
    else
        sig_dir[2] = dirz;            
    p[2] = 0;
  }
  else
    sig_step[2] = 0;

  diff = curr.a - prv.a;
  p[3] = p[3] + diff;
  if((abs(p[3])) >= micronperpulseA)
  {
    sig_step[3] = 1;
    if(p[3]<0)
      sig_dir[3] = !dira;
    else
        sig_dir[3] = dira;            
    p[3] = 0;
  }
  else
    sig_step[3] = 0;

  diff = curr.b - prv.b;
  p[4] = p[4] + diff;
  if((abs(p[4])) >= micronperpulseB)
  {
    sig_step[4] = 1;
    if(p[4]<0)
      sig_dir[4] = !dirb;
    else
        sig_dir[4] = dirb;            
    p[4] = 0;
  }
  else
    sig_step[4] = 0;
    
  diff = curr.c - prv.c;
  p[5] = p[5] + diff;
  if((abs(p[5])) >= micronperpulseC)
  {
    sig_step[5] = 1;
    if(p[5]<0)
      sig_dir[5] = !dirc;
    else
        sig_dir[5] = dirc;            
    p[5] = 0;
  }
  else
    sig_step[5] = 0;
  
  //display to chek errors
  /*
  if((abs(curr.x - prv.x)) > 1)
    SerialUSB.println(curr.x - prv.x);
    if((abs(curr.y - prv.y)) > 1)
      SerialUSB.println(curr.y - prv.y);
    if((abs(curr.z - prv.z)) > 1)
      SerialUSB.println(curr.z - prv.z);
    if((abs(curr.a - prv.a)) > 1)
      SerialUSB.println(curr.a - prv.a);
    if((abs(curr.b - prv.b)) > 1)
      SerialUSB.println(curr.b - prv.b);
    if((abs(curr.c - prv.c)) > 1)
      SerialUSB.println(curr.c - prv.c );
   */
  //set feed rate here according to feed rate read from currect fifo data memeber
  /*
      time period for pulses = 10^6 / (speed) microSecond
   */

  newPeriod = 1000000/(curr.Speed);
  reAssignTimers(newPeriod,pulseWidth);  
  //SerialUSB.print(newPeriod);
  //SerialUSB.println(":uS period");
}

void handler_period()
{
  //send direction and then step
  digitalWrite(DIRPinX,sig_dir[0]);
  digitalWrite(DIRPinY,sig_dir[1]);  
  digitalWrite(DIRPinZ,sig_dir[2]);
  digitalWrite(DIRPinA,sig_dir[3]);
  digitalWrite(DIRPinB,sig_dir[4]);
  digitalWrite(DIRPinC,sig_dir[5]);

  digitalWrite(STEPPinX,sig_step[0]);
  digitalWrite(STEPPinY,sig_step[1]);
  digitalWrite(STEPPinZ,sig_step[2]);
  digitalWrite(STEPPinA,sig_step[3]);
  digitalWrite(STEPPinB,sig_step[4]);
  digitalWrite(STEPPinC,sig_step[5]);  
  //code used while debugging. pos is a global parameter showing how many steps has x moved wrt origin
  /*
  if(sig_step[0] == 1 || sig_step[1] == 1)
  {
    SerialUSB.print("@x ");
    SerialUSB.print((int)sig_step[0]);
    SerialUSB.print((int)sig_dir[0]);
    SerialUSB.print(" @y ");
    SerialUSB.print((int)sig_step[1]);
    SerialUSB.println((int)sig_dir[1]);
  }*/
}

void pausePulsing()
{
  Timer2.pause();
}

void resumePulsing()
{
  Timer2.resume();
}



